#include "tracker.h"
#include "modulator.h"
#include <iostream>
#include <fstream>
#include <iomanip>
#include "win_exception.h"
#include "compensator.h"

using namespace std;
using namespace hard;
using namespace cv;

const char ESC_KEY = 27;

struct image_selection_t
{
  Mat image;
  Point origin;
  Rect selection;
  bool selecting;
  bool ready;

  image_selection_t(): selecting(false), ready(false) {}
};

LPSTR loadAudioBlock(const char* filename, DWORD* blockSize)
{
  HANDLE hFile= INVALID_HANDLE_VALUE;
  DWORD size = 0;
  DWORD readBytes = 0;
  void* block = NULL;
  /*
   * open the file
   */
  if((hFile = CreateFile(
    filename,
    GENERIC_READ,
    FILE_SHARE_READ,
    NULL,
    OPEN_EXISTING,
    0,
    NULL
  )) == INVALID_HANDLE_VALUE)
  return NULL;
  /*
   * get it's size, allocate memory and read the file
   * into memory. don't use this on large files!
   */
  do {
  if((size = GetFileSize(hFile, NULL)) == 0) 
  break;
  if((block = HeapAlloc(GetProcessHeap(), 0, size)) == NULL)
  break;
  ReadFile(hFile, block, size, &readBytes, NULL);
  } while(0);
  CloseHandle(hFile);
  *blockSize = size;
  return (LPSTR)block;
}

int fun1()
{
  LPSTR block;/* pointer to the block */
  DWORD blockSize;/* holds the size of the block */
  if((block = loadAudioBlock("teste.raw", &blockSize)) == NULL)
    fprintf(stderr, "Unable to load file\n");
  
  LPSTR block2;/* pointer to the block */
  DWORD blockSize2;/* holds the size of the block */
  if((block2 = loadAudioBlock("teste.raw", &blockSize2)) == NULL)
    fprintf(stderr, "Unable to load file\n");


  DWORD nSamplesPerSec=44100;

  ofstream out_file("samples.txt");
  win_raw_sound_ostream out(nSamplesPerSec,16,1);
  
  const size_t samples_size = 1000;
  short samples[samples_size];
  float amplitude = 1;
  float next_factor = 1.0594630943592952645618252949463;
  float freq = 440; // A
  float pi = 3.14159265;
  float omega = freq*pi*2;

  for(int n=0; n<50; ++n)
  {
    for(int i=0; i<samples_size; ++i)
      out_file<<(samples[i]= 32768 * amplitude * sin((n*samples_size + i)*(omega/nSamplesPerSec)))<<endl;
    out.write((char*)samples,samples_size);
    amplitude -= 1./50;
    cout<<amplitude<<endl;
  }

  //out.write(block2,blockSize2);

  cout<<endl<<sizeof(short)<<endl<<
    out.QUEUE_EMPTY<<endl<<
    MMSYSERR_NOERROR<<endl<<
    MMSYSERR_ALLOCATED<<endl<<
    MMSYSERR_BADDEVICEID<<endl<<
    MMSYSERR_NODRIVER<<endl<<
    MMSYSERR_NOMEM<<endl<<
    WAVERR_BADFORMAT<<endl<<
    WAVERR_SYNC<<endl<<
    MMSYSERR_INVALHANDLE<<endl<<
    MMSYSERR_NODRIVER<<endl<<
    MMSYSERR_NOMEM<<endl<<
    WAVERR_UNPREPARED<<endl<<
    WAVERR_STILLPLAYING<<endl<<
    MMSYSERR_NOERROR<<endl;
  
  //system("pause");

  return 0;
}

void onMouse(int event, int x, int y, int, void* arg)
{
  image_selection_t* is = (image_selection_t*) arg;

  if(is->selecting)
  {
    is->selection.x = MIN(x, is->origin.x);
    is->selection.y = MIN(y, is->origin.y);
    is->selection.width = std::abs(x - is->origin.x);
    is->selection.height = std::abs(y - is->origin.y);

    is->selection &= Rect(0, 0, is->image.cols, is->image.rows);
  }

  switch(event)
  {
  case CV_EVENT_LBUTTONDOWN:
    is->origin = Point(x,y);
    is->selection = Rect(x,y,0,0);
    is->selecting = true;
    break;
  case CV_EVENT_LBUTTONUP:
    is->selecting = false;
    if( is->selection.width > 0 && is->selection.height > 0 )
        is->ready = true;
    break;
  }
}

size_t period = 30;
tracker sensor(period,true);
modulator mod(period, 6000, 0);

struct y_sensor
{  
  double operator()()
  {
    return sensor.pos().second;
  }
};

struct am_actuator
{
  double operator()(double contr_signal)
  {
    double A = 0.82 + 0.18*contr_signal;
    mod.set_amplitude(A);
    return A;
  }
};

int fun3()
{
  X_WIN_THROW_IF_NULL(SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS), "main.cpp SetPriorityClass");

  ofstream outf("points.txt");

  saturator<PID<y_sensor,am_actuator> > controller;

  chronometer chr(period);

  if(!sensor.get_cam().isOpened())
    return 6661;

  image_selection_t is;
  cvNamedWindow("floater");
  setMouseCallback("floater", onMouse, &is);

  cvNamedWindow("trackers",0);
  int vmin = 10;
  createTrackbar( "Vmin", "trackers", &vmin, 256, 0 );
  int vmax = 256;
  createTrackbar( "Vmax", "trackers", &vmax, 256, 0 );
  int smin = 30;
  createTrackbar( "Smin", "trackers", &smin, 256, 0 );
  int ybias = 906;
  createTrackbar( "bias", "trackers", &ybias, 1000, 0 );
  int yfs = 164;
  createTrackbar( "f.s.", "trackers", &yfs, 1000, 0 );
  int Kp = 580;
  createTrackbar( "Kp", "trackers", &Kp, 1000, 0 );
  int Ki = 23;
  createTrackbar( "Ki", "trackers", &Ki, 1000, 0 );
  int Kd = 105;
  createTrackbar( "Kd", "trackers", &Kd, 1000, 0 );
  int setpoint = 0;
  createTrackbar( "setpoint", "trackers", &setpoint, 1000, 0 );

  cvNamedWindow("histogram");

  RotatedRect trackBox;
  Mat frame;

  char key;
  bool backproj_mode=false;
  bool closed_loop_mode=false;
  while(ESC_KEY != (key=waitKey(1)))
  {
    chr.start();

    sensor.get_camshift_wrapper().set_vmin(vmin);
    sensor.get_camshift_wrapper().set_vmax(vmax);
    sensor.get_camshift_wrapper().set_smin(smin);
    
    sensor.adjust_bias(Point2f(sensor.get_bias().x, static_cast<double>(ybias)/1000.));
    sensor.adjust_full_scale(Point2f(sensor.get_full_scale().x, static_cast<double>(yfs)/1000.));
    
    controller.setpoint(static_cast<double>(setpoint)/1000.);
    controller.Kp(static_cast<double>(Kp)/1000.);
    controller.Ki(static_cast<double>(Ki)/1000000.);
    controller.Kd(static_cast<double>(Kd)/1.);

    switch(key)
    {
    case 'b':
      backproj_mode = !backproj_mode;
      break;
    case 'i':
      controller.zero_int();
      break;
    case 'o':
      closed_loop_mode = !closed_loop_mode;
      break;
    case 'q':
      setTrackbarPos("Kp", "trackers", --Kp);
      break;
    case 'w':
      setTrackbarPos("Kp", "trackers", ++Kp);
      break;
    case 'a':
      setTrackbarPos("Ki", "trackers", --Ki);
      break;
    case 's':
      setTrackbarPos("Ki", "trackers", ++Ki);
      break;
    case 'z':
      setTrackbarPos("Kd", "trackers", --Kd);
      break;
    case 'x':
      setTrackbarPos("Kd", "trackers", ++Kd);
      break;
    case ',':
      setTrackbarPos("setpoint", "trackers", --setpoint);
      break;
    case '.':
      setTrackbarPos("setpoint", "trackers", ++setpoint);
      break;
    case ' ':
      setTrackbarPos("setpoint", "trackers", (setpoint=500));
      break;
    }

    is.image = sensor.get_frame();
    if(is.image.empty())
      return 6662; 
    
    if(is.ready)
    {
      sensor.start_tracking(is.image, is.selection);
      is.ready=false;
      imshow("histogram", sensor.get_camshift_wrapper().hist_image());
    }

    if(is.selecting && is.selection.width > 0 && is.selection.height > 0 )
    {
        Mat roi(is.image, is.selection);
        bitwise_not(roi, roi);
    }

    if(sensor.get_camshift_wrapper().started())
    {
      trackBox = sensor.get_track_box();

      if(backproj_mode)
        cvtColor(sensor.get_camshift_wrapper().probability_image(), is.image, CV_GRAY2BGR);

      if(sensor.get_camshift_wrapper().lost())
        cout<<"lost object\n";
      else
      {
        //cout<<"tracked object\n";
        //cout<<trackBox.size.width<<"\t\t"<<trackBox.size.height<<endl;
        //cout<<(trackBox.size.width==trackBox.size.width)<<"\t\t"<<(trackBox.size.height==trackBox.size.height)<<endl;
        ellipse(is.image, trackBox, Scalar(0,0,255), 3, CV_AA);
        
        double sp = controller.setpoint();
        double pos = sensor.pos().second;
        double contr_signal = controller.signal();
        double A = mod.get_amplitude();
        cout<<showpos<<setw(10)<<"position:  "<<pos<<"\tcontroller: "<<contr_signal<<"\tamplitude: "<<A<<endl;
        outf<<showpos<<setw(10)<<static_cast<double>(clock())/CLOCKS_PER_SEC<<"\t\t"<<
          sp<<"\t\t"<<pos<<"\t\t"<<contr_signal<<"\t\t"<<A<<endl;
      }
    }
    imshow("floater", is.image);

    if(closed_loop_mode)
    {
      if(sensor.get_camshift_wrapper().started())
          controller.start();
    }
    else
    {
      controller.stop();
      mod.set_amplitude(static_cast<double>(setpoint)/1000.);
    }

    chr.stop();
  }

  cvDestroyWindow("floater");
  cvDestroyWindow("trackers");
  cvDestroyWindow("histogram");

  cout<<"main      mean cycle:     "<<chr.mean_ms()<<"ms\n";
  cout<<"main      cycles/overrun: "<<chr.cycles<<"/"<<chr.overrun<<endl;
  cout<<"tracker   mean cycle:     "<<sensor.get_chronometer().mean_ms()<<"ms\n";
  cout<<"tracker   cycles/overrun: "<<sensor.get_chronometer().cycles<<"/"<<sensor.get_chronometer().overrun<<endl;
  cout<<"cam       mean cycle:     "<<sensor.get_cam_chronometer().mean_ms()<<"ms\n";
  cout<<"cam       cycles/overrun: "<<sensor.get_cam_chronometer().cycles<<"/"<<sensor.get_cam_chronometer().overrun<<endl;
  cout<<"modulator mean cycle:     "<<mod.get_chronometer().mean_ms()<<"ms\n";
  cout<<"modulator cycles/overrun: "<<mod.get_chronometer().cycles<<"/"<<mod.get_chronometer().overrun<<endl;
  cout<<"stream    mean cycle:     "<<mod.get_win_raw_sound_ostream().get_chronometer().mean_ms()<<"ms\n";
  cout<<"stream    cycles/overrun: "<<mod.get_win_raw_sound_ostream().get_chronometer().cycles
    <<"/"<<mod.get_win_raw_sound_ostream().get_chronometer().overrun<<endl;
  
  return sensor.get_chronometer().overrun;
}

int fun4()
{
  size_t t = 200;
  size_t t2 = 100;
  modulator m(t2,0);

  //for(int i=0; i<10; ++i)
  //{
  //  m.set_freq(m.freq_A);
  //  Sleep(t);  
  //  m.set_freq(m.freq_As);
  //  Sleep(t);  
  //}
  
  // 

  m.set_freq(m.freq_B*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_B*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_C*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_D*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_D*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_C*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_B*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_A*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_B*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_B*2);
  Sleep(t);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_A*2);
  Sleep(t);
m.set_freq(0); Sleep(t2);
  m.set_freq(m.freq_A*2);
  Sleep(t);
  Sleep(t);
  

  return 0;
}

int fun5()
{
  VideoCapture cam(CV_CAP_ANY);
  
  Mat frame, hsv_frame, thresholded;
  Scalar hsv_min = cvScalar(160, 100, 90, 0);
  Scalar hsv_max = cvScalar(190, 190, 150, 0);

  namedWindow("teste rough circle");

  bool back_mode=false;
  char key;
  while(ESC_KEY != (key=waitKey(1)))
  {
    cam >> frame;
    cvtColor(frame, hsv_frame, CV_BGR2HSV);
    //inRange(hsv_frame, hsv_min, hsv_max, thresholded);
    cvtColor(hsv_frame, thresholded, CV_RGB2GRAY);
    equalizeHist( thresholded, thresholded);
    GaussianBlur( thresholded, thresholded, Size(9, 9), 2, 2 );
    vector<Vec3f> circles;
    HoughCircles(thresholded, circles, CV_HOUGH_GRADIENT,
      2, thresholded.rows/4, 100, 50, 10, 400 );
    
    if(key=='b')
      back_mode = !back_mode;

    if(back_mode)
      frame = thresholded;

    //vector<Mat> ch;
    //split(hsv_frame, ch);
    //frame = ch[0];
    //Mat hist;
    //int hsize = 16;
    //float hranges[] = {0, 180};
    //float sranges[] = { 0, 256 };
    //float vranges[] = { 0, 256 };
    //const float* ranges[] = { hranges, sranges, vranges };
    //int hsv_frame_chanels[] = { 0, 1, 2 };

    //calcHist(&hsv_frame, 1, hsv_frame_chanels, Mat(), hist, 1, &hsize, ranges);
    //normalize(hist, hist, 0, 255, CV_MINMAX);

    //for(int i=0; i<hsize; ++i)
    //  cout<<hist.at<float>(i)<<endl;

    for( size_t i = 0; i < circles.size(); i++ )
    {
         Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
         int radius = cvRound(circles[i][2]);
         // draw the circle center
         circle( frame, center, 3, Scalar(0,255,0), -1, 8, 0 );
         // draw the circle outline
         circle( frame, center, radius, Scalar(0,0,255), 3, 8, 0 );
    }

    imshow("teste rough circle", frame);
  }

  cvDestroyWindow("teste rough circle");

  return 0;
}


int main()
{
  int ret=-1;
  try {
    ret = fun3();
  }
  catch(exception& e)
  {
    cout<<endl<<"error: "<<e.what()<<endl;
  }
  cout<<ret<<endl;
  system("pause");
  return ret;
}